/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.grt192.mechanism.cannonbot;

import com.grt192.actuator.GRTTwoWaySolenoid;
import com.grt192.core.Mechanism;
import com.grt192.mechanism.GRTCompressor;
import com.grt192.core.Command;

/**
 * 
 * @author nikhil
 */
public class CBArm extends Mechanism {

	public static final int SLEEP_TIME = 50;
	public static final int TIMEOUT_LENGTH = 2000;

	private GRTTwoWaySolenoid extenderSolenoid;
	private GRTCompressor compressor;
	private boolean extended;
	private boolean toggling;

	public CBArm(int exsolpin1, int exsolpin2, GRTCompressor compressor) {
		this(new GRTTwoWaySolenoid(exsolpin1, exsolpin2), compressor);
	}

	public CBArm(GRTTwoWaySolenoid exsol, GRTCompressor compressor) {
		extended = toggling = false;
		this.extenderSolenoid = exsol;
		extenderSolenoid.start();
		this.compressor = compressor;
		if(!compressor.isStarted())
			compressor.startCompressor();
		addActuator("exsol", extenderSolenoid);
	}

	public boolean isExtended() {
		return extended;
	}

	// Actions

	public void toggleArm() {
		long startTime = System.currentTimeMillis();
		toggling = true;

		if (extended) {
			retract();// Change
			// if
			// opposite
		} else {
			retract(); // Change
			// if
			// opposite
		}
		
	}

	public void extend() {
		extenderSolenoid.enqueueCommand(GRTTwoWaySolenoid.REVERSE); // Change
		extended = true;
	}

	public void retract() {
		extenderSolenoid.enqueueCommand(GRTTwoWaySolenoid.FORWARD); // Change
		extended = false;
	}

        public void punch() {
            extenderSolenoid.enqueueCommand(new Command(GRTTwoWaySolenoid.REVERSE, 3000)); //punch
            extenderSolenoid.enqueueCommand(GRTTwoWaySolenoid.FORWARD);//retract
            extended = false;
            
        }


	protected void block() {
		try {
			Thread.sleep(SLEEP_TIME);
		} catch (InterruptedException ie) {
			ie.printStackTrace(); // if this happens i wanna know pretty
			// badly.
		}
	}

	public boolean isToggling() {
		return toggling;
	}

}
